#include <stddef.h>
#include "sm_trace.h"
#include "common.h"

void asnWriteInt16(unsigned char* pbBuf, unsigned short wValue)
{
	pbBuf[0] = (unsigned char)(wValue >> 8);
	pbBuf[1] = (unsigned char)wValue;
}

unsigned short asnReadInt16(const unsigned char* pcbBuf)
{
	return ((unsigned short)pcbBuf[0]<< 8) | pcbBuf[1];
}

void asnWriteInt24(unsigned char* pbBuf, unsigned long dwValue)
{
	pbBuf[0] = (unsigned char)(dwValue >> 16);
	pbBuf[1] = (unsigned char)(dwValue >> 8);
	pbBuf[2] = (unsigned char)dwValue;
}

unsigned long asnReadInt24(const unsigned char* pcbBuf)
{
	return ((unsigned long)pcbBuf[0]<< 16) | ((unsigned long)pcbBuf[1]<< 8) | pcbBuf[2];
}


void asnWriteInt32(unsigned char* pbBuf, unsigned long dwValue)
{
	pbBuf[0] = (unsigned char)(dwValue >> 24);
	pbBuf[1] = (unsigned char)(dwValue >> 16);
	pbBuf[2] = (unsigned char)(dwValue >> 8);
	pbBuf[3] = (unsigned char)dwValue;
}

unsigned long asnReadInt32(const unsigned char* pcbBuf)
{
	return ((unsigned long)pcbBuf[0]<< 24) | ((unsigned long)pcbBuf[1]<< 16)
		| ((unsigned long)pcbBuf[2]<< 8) | pcbBuf[3];
}

unsigned char asnWriteApduLengthField(unsigned char* pbBuf, unsigned long dwLengthFieldValue)
{
	unsigned char	bBufLen;

	if (dwLengthFieldValue <= 0x7F)
	{
		if (pbBuf)
			pbBuf[0] = (unsigned char)dwLengthFieldValue;
		bBufLen = 1;
	}
	else if (dwLengthFieldValue <= 0xFF)
	{
		if (pbBuf)
		{
			pbBuf[0] = 1 | 0x80;
			pbBuf[1] = (unsigned char)dwLengthFieldValue;
		}
		bBufLen = 2;
	}
	else if (dwLengthFieldValue <= 0xFFFF)
	{
		if (pbBuf)
		{
			pbBuf[0] = 2 | 0x80;
			pbBuf[1] = (unsigned char)((dwLengthFieldValue >> 8) & 0xFF);
			pbBuf[2] = (unsigned char)dwLengthFieldValue;
		}
		bBufLen = 3;
	}
	else if (dwLengthFieldValue <= 0xFFFFFF)
	{
		if (pbBuf)
		{
			pbBuf[0] = 3 | 0x80;
			pbBuf[1] = (unsigned char)((dwLengthFieldValue >> 16) & 0xFF);
			pbBuf[2] = (unsigned char)((dwLengthFieldValue >> 8) & 0xFF);
			pbBuf[3] = (unsigned char)dwLengthFieldValue;
		}
		bBufLen = 4;
	}
	else
	{
		if (pbBuf)
		{
			pbBuf[0] = 4 | 0x80;
			pbBuf[1] = (unsigned char)((dwLengthFieldValue >> 24) & 0xFF);
			pbBuf[2] = (unsigned char)((dwLengthFieldValue >> 16) & 0xFF);
			pbBuf[3] = (unsigned char)((dwLengthFieldValue >> 8) & 0xFF);
			pbBuf[4] = (unsigned char)dwLengthFieldValue;
		}
		bBufLen = 5;
	}

	return bBufLen;
}

unsigned char asnReadApduLengthField(const unsigned char* pcbBuf, unsigned long dwBufSize,
											unsigned long* pdwLengthFieldValue)
{
	unsigned char bLen;
	unsigned char i;

	if (pcbBuf[0] <= 0x7F)
	{
		if (pdwLengthFieldValue)
			*pdwLengthFieldValue = pcbBuf[0];
		bLen = 1;
	}
	else
	{
		bLen = (pcbBuf[0] & 0x7F) + 1;
		if (bLen <= dwBufSize)
		{
			if (pdwLengthFieldValue)
			{
				*pdwLengthFieldValue = 0;
				for (i = 1; i < bLen; i++)
					*pdwLengthFieldValue = (*pdwLengthFieldValue << 8) | pcbBuf[i];
			}
		}
		else
		{
			if (pdwLengthFieldValue)
				*pdwLengthFieldValue = 0;
			bLen = (unsigned char)dwBufSize;
		}
	}

	return bLen;
}


unsigned char* decode_apdu(unsigned char* pbApdu,
						   unsigned long* pdwApduTag,
						   unsigned long* pdwLengthField)
{
	//analysis the apdu pointed by pbApdu.
	//parameter:
	//	pbApdu: first byte address of the apdu.
	//	pdwApduTag: address where apdutag value should be returned to.
	//	pdwLengthField: address the length value of the data field should be returned to.
	//return value:
	//	NULL: parameter value error
	//	>0	: the first byte address of the data field of the apdu pointed by pbApdu.

	int		sizeLenfield;

	SM_TRACE_ENTRY(TRMOD_APP, "decode_apdu() - entry,pbApdu=0x%08X, pdwApduTag=0x%08X,"
		"pdwLengthField=0x%08X\r\n", pbApdu, pdwApduTag, pdwLengthField);

	if((NULL==pbApdu)||(NULL==pdwApduTag)||(NULL==pdwLengthField))
	{
		SM_TRACE_ERROR(TRMOD_APP, "decode_apdu() - invalid parameter\r\n");
		return NULL;
	}

	*pdwApduTag = asnReadInt24(pbApdu);
	pbApdu+=3;

	sizeLenfield = asnReadApduLengthField(pbApdu, -1, pdwLengthField);
	pbApdu += sizeLenfield;

	SM_TRACE_RETURN(TRMOD_APP, "decode_apdu() - return, apduTag=0x%06X,"
		"lengthField=%d,pbApdu=0x%08X\r\n",
		*pdwApduTag, *pdwLengthField, pbApdu);

	return pbApdu;
}
